#include "ImageDeal.h"
#include "include.h"

#define Nofindedthreshold 10    //丢灯判定的帧数

/**
 * @Date2022-05-13 14:46:30
 * @decription     
 * @author         Tennikl
 * @versions       Ver 0.0
 * @param {uint8_t} *A      -- Image date[H][W] 图像数据
 * @param {uint8_t} H       -- Image high 图像高度
 * @param {uint8_t} W       -- Image wide 图像宽度
 * @param {uint8_t} *Loc    -- Loction of lamp 灯的坐标
 * @return {*}
 */
void Seek_Beacon_Teny(uint8_t *A, uint8_t H, uint8_t W ,uint8_t *Loc)
{
    uint8_t tm = 0;
    uint8_t nr = 0; //行
    uint8_t nc = 0; //列
    static int sumcol;
    static int sumrow;
    static uint8_t NofindedCount;
    uint8_t dotcol[300] = {0};
    uint8_t dotrow[300] = {0};

    NofindedCount = 0;
    dotcnt = 0;
    for (nr = H - 1; nr > 0; --nr)
    {
        for (nc = (W>>1); nc > 0; --nc)
        {
            if (*(A + nr * W + nc)!=0)
            {
                dotrow[dotcnt] = nr;   // 记录所有的白点水垂直向方位，也就是远近
                dotcol[dotcnt++] = nc; // 记录所有的白点水平方向方位，也就是左右偏差
            }
            // // 188*120分辨率下，有两个连续白点，则认为是可能的信标灯
            // if ((*(A + (nr - 1) * W + nc) + *(A + (nr + 1) * W + nc) + *(A + nr * W + nc + 1) + *(A + nr * W + nc - 1) > 500))
            // {
            //     dotrow[dotcnt] = nr;   // 记录所有的白点水垂直向方位，也就是远近
            //     dotcol[dotcnt++] = nc; // 记录所有的白点水平方向方位，也就是左右偏差
            // }
        }
        for (nc = (W>>1) + 1 ; nc < W - 1; ++nc)
        {
            if (*(A + nr* W + nc)!=0)
            {
                dotrow[dotcnt] = nr;   // 记录所有的白点水垂直向方位，也就是远近
                dotcol[dotcnt++] = nc; // 记录所有的白点水平方向方位，也就是左右偏差
            }
            // // 188*120分辨率下，有两个连续白点，则认为是可能的信标灯
            // if ((*(A + (nr - 1) * W + nc) + *(A + (nr + 1) * W + nc) + *(A + nr * W + nc + 1) + *(A + nr * W + nc - 1) > 500))
            // {
            //     dotrow[dotcnt] = nr;   // 记录所有的白点水垂直向方位，也就是远近
            //     dotcol[dotcnt++] = nc; // 记录所有的白点水平方向方位，也就是左右偏差
            // }
        }
    }

    if (dotcnt) // 发现有白斑，更新数据
    {
        NofindedCount = 0;
        sumcol = 0, sumrow = 0; // 清除上次的结果
        for (tm = 0; tm < dotcnt; tm++)
        {
            sumcol += dotcol[tm]; // 所有白点左右偏差值求和
            sumrow += dotrow[tm]; // 所有白点上下偏差值求和，暂时没用到
        }
    }
    else if (NofindedCount < Nofindedthreshold)
    {
        ++NofindedCount;
    }
    else if (NofindedCount >= Nofindedthreshold)
    {
        sumcol = 0, sumrow = 0;
    }

    *Loc = sumcol / dotcnt; // 灯的左右中心点
    *(Loc + 1) = sumrow / dotcnt; // 灯的远近中心点，暂时没用到
    return;
}

/**
 * @Date2022-07-18 14:10:41
 * @decription     画个十字
 * @author         Tennikl
 * @versions       Ver 0.0
 * @param {uint8_t} *Img    Image data[H][W]
 * @param {uint8_t} H       Image high
 * @param {uint8_t} W       Image width
 * @param {uint8_t} *Loc    Loction of lamp
 * @return {*}
 */
void Draw(uint8_t *Img, uint8_t H, uint8_t W, uint8_t *Loc)
{
    if((*(Loc))!=0)
    {
        for (int i = 0; i < H; ++i)
            *(Img + i * W + *(Loc)) = 255;
        for (int i = 0; i < W; ++i)
            *(Img + W * (*(Loc + 1)) + i) = 255;
        *(Img + W * (*(Loc + 1)) + *(Loc)) = 0;
        
        if(*(Loc)>=2)
        {
            if (*(Loc + 1) >= 2)        
            {
                *(Img + W * (*(Loc + 1) - 1) + *(Loc)-1) = 255;
                *(Img + W * (*(Loc + 1) - 2) + *(Loc)-1) = 255;
                *(Img + W * (*(Loc + 1) - 1) + *(Loc)-2) = 255;
                *(Img + W * (*(Loc + 1) - 2) + *(Loc)-2) = 255;
            }
            if (*(Loc + 1) <= H - 2)    
            {
                *(Img + W * (*(Loc + 1) + 1) + *(Loc)-1) = 255;
                *(Img + W * (*(Loc + 1) + 2) + *(Loc)-1) = 255;
                *(Img + W * (*(Loc + 1) - 1) + *(Loc)-2) = 255;
                *(Img + W * (*(Loc + 1) + 1) + *(Loc)-2) = 255;
            }
        }
        if(*(Loc)<=W - 3)
        {
            // if (*(Loc + 1) >= 1)        *(Img + W * (*(Loc + 1) - 1) + *(Loc)+1) = 255;
            // if (*(Loc + 1) <= H - 2)    *(Img + W * (*(Loc + 1) + 1) + *(Loc)+1) = 255;

            if (*(Loc + 1) >= 2)        
            {
                *(Img + W * (*(Loc + 1) - 1) + *(Loc)+1) = 255;
                *(Img + W * (*(Loc + 1) - 2) + *(Loc)+1) = 255;
                *(Img + W * (*(Loc + 1) - 1) + *(Loc)+2) = 255;
                *(Img + W * (*(Loc + 1) - 2) + *(Loc)-2) = 255;
            }
            if (*(Loc + 1) <= H - 2)    
            {
                *(Img + W * (*(Loc + 1) + 1) + *(Loc)+1) = 255;
                *(Img + W * (*(Loc + 1) + 2) + *(Loc)+1) = 255;
                *(Img + W * (*(Loc + 1) - 1) + *(Loc)+2) = 255;
                *(Img + W * (*(Loc + 1) + 1) + *(Loc)+2) = 255;
            }
        }

        


        
        
        *(Img + W*(*(Loc+1)+1) + *(Loc)-1) = 255;
        *(Img + W*(*(Loc+1)+1) + *(Loc)+1) = 255;
        *(Img + W*(*(Loc+1)-1) + *(Loc)-1) = 255;
        *(Img + W*(*(Loc+1)-1) + *(Loc)+1) = 255;
    }
}


/**
 * @Date2022-05-11 16:37:38
 * @decription     固定阈值二值化
 * @author         Tennikl
 * @versions       Ver 0.0
 * @param   {uint8_t*}  Data    数据地址
 * @param   {int}       lenth   数据长度
 * @return {*}
 */
void Binaryzation(uint8_t * Data, int lenth)
{
    int threshold = FixedThreshold;
    for(int i = threshold; i<lenth; ++i)
    {
        if(*(Data+i)<=threshold)    *(Data+i) = 0;
        else                        *(Data+i) = 255;
    }
}

/**
 * @Date2022-05-11 15:50:33
 * @decription     图像主函数
 * @author         Tennikl
 * @versions       Ver 0.0
 * @param {*}
 * @return {*}
 */
void ImageDeal()
{
    //int Location[2] = {0};//寻灯坐标
    //int dir = 0;
    Binaryzation((uint8_t *)Image_Data, IMAGEH * IMAGEW);//固定阈值二值化
    
    //提取部分使用的数据(显示用)
    Get_Use_Image();  //裁剪（提取显示中心128*64）: 得到 Image_Use
    //Get_Bin_Image(0); //动态阈值（大津法）二值化: 对Image_Use 二值化

    //寻找灯坐标并返回 Location[2]
    Seek_Beacon_Teny((uint8_t *)Image_Use, LCDH, LCDW, Location);//寻找灯坐标
    Draw((uint8_t *)Image_Use, LCDH, LCDW, Location);//十字标出坐标点

    //返回坐标对应的dir
    if(count_1s>=5 && (Global_mode == 0||Global_mode==6))
        dir_judge(Location, &target_speed, &pwm_dir);

    return;
}




